import rclpy
from sentry_interfaces.msg import RobotHP
from rm_interfaces.msg import ReceiveData,SendData
from geometry_msgs.msg import Twist
from rclpy.node import Node
from std_msgs.msg import String,Float32
from geometry_msgs.msg import TransformStamped
from tf2_ros import TransformBroadcaster
from tf2_ros import StaticTransformBroadcaster
from tf2_ros import TransformStamped
from geometry_msgs.msg import Quaternion
from math import pi
import math

def euler_to_quaternion(roll, pitch, yaw):
    cy = math.cos(yaw * 0.5)
    sy = math.sin(yaw * 0.5)
    cp = math.cos(pitch * 0.5)
    sp = math.sin(pitch * 0.5)
    cr = math.cos(roll * 0.5)
    sr = math.sin(roll * 0.5)

    w = cy * cp * cr + sy * sp * sr
    x = cy * cp * sr - sy * sp * cr
    y = sy * cp * sr + cy * sp * cr
    z = sy * cp * cr - cy * sp * sr

    return Quaternion(x=x, y=y, z=z, w=w)

class UnityBridge(Node):

    def __init__(self):
        super().__init__('unity_bridge')
        # unityToRos生命值发布端
        self.publisher_robot_hp = self.create_publisher(RobotHP, 'robot_hp', 10)
        self.timer_robot_hp = self.create_timer(1.0, self.publish_RobotHP_message)
        # unityToRos陀螺仪发布端
        self.publisher_serial_sub_ = self.create_publisher(ReceiveData, 'serial_sub_', 10)
        self.timer_serial_sub_ = self.create_timer(0.001, self.publish_ReceiveData_message)
        # rosToUnity云台控制发布端
        self.publisher_cmd_gimbal = self.create_publisher(Twist, 'cmd_gimbal', 10)
        self.timer_cmd_gimbal = self.create_timer(1.0, self.publish_Twist_message)
        # unityToRos生命值接收端
        self.subscription_mate_hp = self.create_subscription(String,'mate_hp_unity',self.mate_hp_unity_callback,10)
        self.subscription_mate_hp  # 防止被垃圾回收
        self.subscription_enemy_hp = self.create_subscription(String,'enemy_hp_unity',self.enemy_hp_unity_callback,10)
        self.subscription_enemy_hp
        # unityToRos陀螺仪接收端
        self.subscription_pitch_info = self.create_subscription(Float32,'pitch_info',self.pitch_info_callback,10)
        self.subscription_pitch_info  # 防止被垃圾回收
        self.subscription_yaw_info = self.create_subscription(Float32,'yaw_info',self.yaw_info_callback,10)
        self.subscription_yaw_info  # 防止被垃圾回收
        # rosToUnity云台控制接收端
        self.subscription_aimtag_data = self.create_subscription(SendData,'serial_pub_',self.aimtag_data_callback,10)
        self.subscription_aimtag_data  # 防止被垃圾回收
        # rosToUnity装甲板坐标发布方
        

        # 变量 serial_pub_
        self.mate_hp_ = [100, 0, 0, 0, 0, 0, 0, 0]
        self.enemy_hp_=[100, 0, 0, 0, 0, 0, 0, 0]
        # 陀螺仪接收
        self.pitch_angle_receiver=0.0
        self.yaw_angle_receiver=0.0
        # 云台控制发送
        self.pitch_angle_sender=0.0
        self.yaw_angle_sender=0.0
        # tf2
        self.static_broadcaster = StaticTransformBroadcaster(self)
        self.dynamic_broadcaster = TransformBroadcaster(self)
        self.timer = self.create_timer(0.001, self.publish_static_transform)
        self.gim_xyz_coeff = [0, 44.5, 98.0]
    
    def publish_static_transform(self):
        ts = TransformStamped()
        ts.header.stamp = self.get_clock().now().to_msg()
        ts.header.frame_id = 'ground'
        ts.child_frame_id = 'gyro'
        ts.transform.translation.x = 0.0
        ts.transform.translation.y = 0.0
        ts.transform.translation.z = 0.45
        ts.transform.rotation = euler_to_quaternion(-pi/2, 0, 0)
        self.static_broadcaster.sendTransform(ts)

        ts = TransformStamped()
        ts.header.stamp = self.get_clock().now().to_msg()
        ts.header.frame_id = 'gyro'
        ts.child_frame_id = 'gimbal'
        ts.transform.translation.x = 0.0
        ts.transform.translation.y = 0.0
        ts.transform.translation.z = 0.0
        ts.transform.rotation = euler_to_quaternion(self.pitch_angle_receiver * pi / 180.0, -self.yaw_angle_receiver * pi / 180.0, 0)
        self.dynamic_broadcaster.sendTransform(ts)

        ts = TransformStamped()
        ts.header.stamp = self.get_clock().now().to_msg()
        ts.header.frame_id = 'gimbal'
        ts.child_frame_id = 'camera'
        ts.transform.translation.x = self.gim_xyz_coeff[0] / 1000.0
        ts.transform.translation.y = self.gim_xyz_coeff[1] / 1000.0
        ts.transform.translation.z = self.gim_xyz_coeff[2] / 1000.0
        ts.transform.rotation = euler_to_quaternion(0, 0, 0)
        self.static_broadcaster.sendTransform(ts)

    def publish_RobotHP_message(self):
        robot_hp_msg = RobotHP()
        robot_hp_msg.mate_hp = self.mate_hp_
        robot_hp_msg.enermy_hp = self.enemy_hp_
        self.publisher_robot_hp.publish(robot_hp_msg)
        #self.get_logger().info('Publishing custom message: %s' % robot_hp_msg)

    def publish_ReceiveData_message(self):
        ReceiveData_msg = ReceiveData()
        ReceiveData_msg.header.stamp = self.get_clock().now().to_msg()
        ReceiveData_msg.pitch_angle = self.pitch_angle_receiver
        ReceiveData_msg.yaw_angle = self.yaw_angle_receiver
        # self.get_logger().info('Publishing custom message: %s' % self.yaw_angle_receiver)
        self.publisher_serial_sub_.publish(ReceiveData_msg)
    
    def publish_Twist_message(self):
        Twist_msg = Twist()
        
        Twist_msg.angular.y=self.pitch_angle_sender
        Twist_msg.angular.z=self.yaw_angle_sender
        
        self.publisher_cmd_gimbal.publish(Twist_msg) 
    
    def mate_hp_unity_callback(self, msg):
        data_list= msg.data.split(";")
        self.mate_hp_=[int(x) for x in data_list]

    def enemy_hp_unity_callback(self, msg):
        data_list= msg.data.split(";")
        self.enemy_hp_=[int(x) for x in data_list]

    def pitch_info_callback(self, msg):
        self.pitch_angle_receiver= msg.data

    def yaw_info_callback(self, msg):
        self.yaw_angle_receiver= msg.data

    def aimtag_data_callback(self, msg):
        self.pitch_angle_sender=msg.data.pitch
        self.yaw_angle_sender=msg.data.yaw
        
        

def main(args=None):
    rclpy.init(args=args)

    UnityBridge_ = UnityBridge()

    rclpy.spin(UnityBridge_)

    UnityBridge_.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()